#!/usr/bin/python3
# -*- coding: utf-8 -*-

import time, Tools, math
import B_DQ4 as B
import numpy as np
import EventLog as log

inJDDstart = {11: 0, 12: 0, 13: 0, 14: 0, 15: 0, 16: 0, 21: 0, 22: 0, 23: 0, 24: 0, 25: 0, 26: 0
        , 31: 0, 32: 0, 33: 0, 34: 0, 35: 0, 36: 0, 37: 0, 41: 0, 42: 0, 43: 0, 44: 0, 45: 0, 46: 0, 47: 0
                  , 71: 0, 72: 0}

def BlancePose1(g=0, h1=20, h2=20, b1=0, b2=0, sp=10,der = [0,0],hand = [0,0]):
    q = 5
    JDDh1 = {13: h1 + b1 - q, 14: h1 * 2, 15: h1 - b1 + q}
    JDDh2 = {23: h2 + b2 - q, 24: h2 * 2, 25: h2 - b2 + q}
    JDDg = {12: g+der[0], 16: -g, 22: -g+der[1], 26: g}
    JDD = {}
    JDD.update(JDDh1)
    JDD.update(JDDh2)
    JDD.update(JDDg)
    JDDh = {32:2,42:2,34:40,44:40,31:hand[0]-0,41:hand[1]-0}
    JDD.update(JDDh)

    P.SetPoseFlowDic(JDD, sleeptime=5, steps=sp)
    inJDDstart.update(JDD)

def BlancePose(g=0, h1=20, h2=20, b1=0, b2=0, sp=10,der = [0,0],hand = [0,0],waist = 0):
    q = 5
    JDDh1 = {13: h1 + b1 - q, 14: h1 * 2, 15: h1 - b1 + q}
    JDDh2 = {23: h2 + b2 - q, 24: h2 * 2, 25: h2 - b2 + q}
    JDDg = {12: g+der[0], 16: -g, 22: -g+der[1], 26: g}
    JDD = {}
    JDD.update(JDDh1)
    JDD.update(JDDh2)
    JDD.update(JDDg)
    JDDh = {32:2,42:2,34:40,44:40,31:hand[0]-20,41:hand[1]-20,72:waist,21:-waist,11:waist}
    JDD.update(JDDh)

    P.SetPoseFlowDic(JDD, sleeptime=5, steps=sp)
    inJDDstart.update(JDD)

    # P.getTouchSensor()

if __name__ == "__main__":
    stepi = 0
    while robot.step(timestep) != -1:
        # robot.step(5000)
        BlancePose(g=0, h1=0, h2=0, b1=0, b2=0, sp=10)  # 起步
        # 起步步态
        stepi = stepi + 1
        log.logger.info("The %sth setp : Start walk pose" % stepi)
        BlancePose(g=0, h1=20, h2=20, b1=0, b2=0, sp=300)  # 起步
        # 第一步：起步迈步
        BlancePose(g=-10, h1=20, h2=20, b1=0, b2=0, sp=300)  # 加速
        BlancePose(g=-10, h1=20, h2=25, b1=0, b2=0, sp=80,hand=[10,-10])  # 抬腿
        BlancePose(g=-10, h1=20, h2=22, b1=0, b2=10, sp=80,hand=[20,-20])  # 落腿
        BlancePose(g=-10, h1=20, h2=20, b1=0, b2=10, sp=140,hand=[23,-23])  # 落腿触地

        stepi = stepi + 1
        log.logger.info("The %sth setp: Normal walk pose L1" % stepi)
        # # 第二步：过度步态
        BlancePose(g=10, h1=13, h2=20, b1=-10, b2=0, sp=251)  # 稳定态
        BlancePose(g=10, h1=30, h2=20, b1=0, b2=0, sp=301, hand=[-10, 10])  # 抬腿
        BlancePose(g=10, h1=20, h2=20, b1=20, b2=0, sp=155, hand=[-20, 20], waist=0)  # 落腿
        BlancePose(g=10, h1=16, h2=20, b1=20, b2=0, sp=70, hand=[-23, 23], waist=0)  # 落腿触地

        while stepi<100:# 需要是偶数
            # 第一步：起步迈步
            stepi = stepi + 1
            log.logger.info("The %sth setp: Normal walk pose R2" % stepi)
            BlancePose(g=-9, h1=25, h2=10, b1=0, b2=-20, sp=251)  # 加速
            BlancePose(g=-9, h1=20, h2=30, b1=0, b2=7, sp=131,hand=[10,-10])  # 抬腿
            BlancePose(g=-9, h1=20, h2=22, b1=0, b2=20, sp=85,hand=[20,-20], waist=-7)  # 落腿
            BlancePose(g=-6, h1=20, h2=20, b1=0, b2=20, sp=20,hand=[23,-23], waist=-7)  # 落腿触地
            stepi = stepi + 1
            log.logger.info("The %sth setp: Normal walk pose L1" % stepi)
            # # 第二步：过度步态
            BlancePose(g=9, h1=10, h2=25, b1=-20, b2=0, sp=251)  # 稳定态
            BlancePose(g=9, h1=30, h2=20, b1=7, b2=0, sp=131, hand=[-10, 10])  # 抬腿
            BlancePose(g=9, h1=22, h2=20, b1=20, b2=0, sp=85, hand=[-20, 20], waist=7)  # 落腿
            BlancePose(g=6, h1=20, h2=20, b1=20, b2=0, sp=20, hand=[-23, 23], waist=7)  # 落腿触地

    # 20230805 大步态稳定行走，速度中等
    # 20230807 还没修改，速度稍微做了一些提升


